#include <verilated.h>
#include <verilated_vcd_c.h>
#include "VErrDiffCorePip.h"
#include "VErrDiffCorePip___024root.h"
#include "debug.h"
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <time.h>
#include <assert.h>
#include <math.h>

#define IMG_H    512
#define IMG_W    512
#define THRES    128
#define STAGE_NR 5

#define AssertExit(cond, format, ...) \
  do { \
    if (!(cond)) { \
      fprintf(stderr, ANSI_FMT(format, ANSI_FG_RED) "\n", ## __VA_ARGS__); \
      show_err_tmp(); \
      exit_sim(); \
      assert(cond); \
    } \
  } while (0)

// global variables
VerilatedContext *contextp = NULL;
VerilatedVcdC *m_trace = NULL;
VErrDiffCorePip *top = NULL;

static int errCache[IMG_H][IMG_W] = {0};
static uint8_t img[IMG_H][IMG_W];
static int err_ring_tmp[4][4] = {0};

static bool
check_bound(int i, int j) {
  return 0 <= i && i < IMG_H && 0 <= j && j < IMG_W;
}

static void
show_err_tmp() {
  for (int i = 0; i < 4; i++) {
    for (int j = 0; j < 4; j++) {
      printf("error_tmp[%d][%d] = %d\n", i, j, err_ring_tmp[i][j]);
    }
  }
}

static uint32_t
random_gen(uint32_t max) {
  srand(rand());
  return rand() % max;
}

/*
 * the default `round` function in <math.h> uses
 * "round half away from zero",
 * but here needs "round half up"(substitution)
 */
static double
round_half_up(double x) {
  return floor(x+0.5);
}

static int
check_bound_then_acc(int i, int j, int val = 0) {
  if (0 <= i && i < IMG_H && 0 <= j && j < IMG_W) {
    errCache[i][j] += val;
    return errCache[i][j];
  }
  return 0;
}

void init_sim() {
  contextp = new VerilatedContext;
  m_trace = new VerilatedVcdC;
  top = new VErrDiffCorePip;
  Verilated::traceEverOn(true);
  top->trace(m_trace, 0);
  m_trace->open("waveform.vcd");
}

void step_and_dump_wave(int n = 1) {
  for (int i = 0; i < n; i++) {
    top->clock ^= 1;
    top->eval();
    contextp->timeInc(1);
    m_trace->dump(contextp->time());

    top->clock ^= 1;
    top->eval();
    contextp->timeInc(1);
    m_trace->dump(contextp->time());
  }
}

void exit_sim() {
  m_trace->close();
  delete top;
}

int main() {
  init_sim();

  // generate random seed
  unsigned int seed = time(0);
  srand(seed);
  // generate gray-image using random generator
  for (int pos = 0; pos < IMG_H*IMG_W; pos++)
    img[pos/IMG_H][pos%IMG_W] = random_gen(0x100);

  // store last diffused error to be read
  int tmpErr = 0;

  // init reset
  top->reset = 1; top->in_valid = 0;
  step_and_dump_wave(5);
  top->reset = 0;

  // trigger hardware
  top->in_valid = 1; step_and_dump_wave();
  top->in_valid = 0;


  // ==================    each stage   ==========================
  for (int pos = 0; pos < IMG_H*IMG_W + STAGE_NR - 1; pos++) {
    // for every stage
    // Get pixel
    uint8_t pix = img[pos/IMG_H][pos%IMG_W];
    top->img_pb_dout = pix;

    // Get right error
    int posn1 = pos - 1;
    if (posn1 >= 0) {
      top->cache_pb_dout = tmpErr; // right
    }
    step_and_dump_wave(); // make a step

    // Verify the binary output
    int posn3 = pos - 3;
    if (posn3 >= 0) {
      uint8_t bval = top->rootp->ErrDiffCorePip__DOT__errorOut__DOT__bval * 0xff;
      AssertExit(bval == img[posn3/IMG_H][posn3%IMG_W], "bval[%#x] != result[%#x], at row[%d] column[%d]", bval, img[posn3/IMG_H][posn3%IMG_W], posn3/IMG_H, posn3%IMG_W);

      // Verify Error Out
      int errOut[4];
      errOut[0] = (int8_t)top->rootp->ErrDiffCorePip__DOT__errorOut__DOT__errOut_0;
      errOut[1] = (int8_t)top->rootp->ErrDiffCorePip__DOT__errorOut__DOT__errOut_1;
      errOut[2] = (int8_t)top->rootp->ErrDiffCorePip__DOT__errorOut__DOT__errOut_2;
      errOut[3] = (int8_t)top->rootp->ErrDiffCorePip__DOT__errorOut__DOT__errOut_3;
      for (int k = 0; k < 4; k++) {
        if (k == 0 && !check_bound(posn3/IMG_H, posn3%IMG_W+1)) {
          continue;
        }
        if (k == 1 && !check_bound(posn3/IMG_H+1, posn3%IMG_W+1)) {
          continue;
        }
        if (k == 2 && !check_bound(posn3/IMG_H+1, posn3%IMG_W)) {
          continue;
        }
        if (k == 3 && !check_bound(posn3/IMG_H+1, posn3%IMG_W-1)) {
          continue;
        }
        AssertExit(errOut[k] == err_ring_tmp[posn3%4][k], "errOut%d[%d] != result[%d], at row[%d] column[%d], current read err is %d", k, errOut[k], err_ring_tmp[posn3%4][k], posn3/IMG_H, posn3%IMG_W, errCache[posn3/IMG_H][posn3%IMG_W]);
      }
    }

    // Store right diffused error, for future reading
    tmpErr = check_bound_then_acc(pos/IMG_H, pos%IMG_W+1); // right

    // Calcutate output binary and errors by software
    img[pos/IMG_H][pos%IMG_W] = pix + errCache[pos/IMG_H][pos%IMG_W] < THRES ? 0 : 0xff;
    int err = pix + errCache[pos/IMG_H][pos%IMG_W] - img[pos/IMG_H][pos%IMG_W];
    int errdiff[4];
    errdiff[0] = (int)round_half_up(err * 7.0/16);
    errdiff[1] = (int)round_half_up(err * 1.0/16);
    errdiff[2] = (int)round_half_up(err * 5.0/16);
    errdiff[3] = (int)round_half_up(err * 3.0/16);

    // Write diffusion error and save to temporary buffer
    err_ring_tmp[pos%4][0] = check_bound_then_acc(pos/IMG_H, pos%IMG_W+1, errdiff[0]);
    err_ring_tmp[pos%4][1] = check_bound_then_acc(pos/IMG_H+1, pos%IMG_W+1, errdiff[1]);
    err_ring_tmp[pos%4][2] = check_bound_then_acc(pos/IMG_H+1, pos%IMG_W, errdiff[2]);
    err_ring_tmp[pos%4][3] = check_bound_then_acc(pos/IMG_H+1, pos%IMG_W-1, errdiff[3]);

    step_and_dump_wave(3);
  }

  OK("Differential tests all passed, safe exit");
  exit_sim();
}
